
namespace Vtl
{
	namespace geometry
	{
		template <typename Scalar, int Dimension>
		inline Scalar pointToLineDistance (const Eigen::Matrix<Scalar,Dimension,1> &_Point,
			const Eigen::ParametrizedLine<Scalar,Dimension> &_Line)
		{
			return sqrtl(sqPointToLineDistance(_Point,_Line));
		}

		template <typename Scalar, int Dimension>
		inline Scalar sqPointToLineDistance (const Eigen::Matrix<Scalar,Dimension,1> &_Point,
			const Eigen::ParametrizedLine<Scalar,Dimension> &_Line)
		{
			typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType;        
			typedef Eigen::Matrix<Scalar, 3, 1> VectorType3D;
			VectorType3D line_dir = common::convertTo3D(_Line.direction(),0.0);
			VectorType3D line_origin = common::convertTo3D(_Line.origin(),0.0);
			VectorType3D point = common::convertTo3D(_Point,0.0);        
			return Scalar(line_dir.cross(line_origin - point).squaredNorm() / line_dir.squaredNorm());
		}

		template <typename Scalar, int Dimension>
		inline Scalar closestPointOnLine(const Eigen::ParametrizedLine<Scalar,Dimension> &_Line, 
			const Eigen::Matrix<Scalar,Dimension,1> &_Point)
		{
			typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType;
			VectorType u = _Line.direction();
			VectorType v = _Point - _Line.origin();
			return Scalar(v.dot(u)/u.dot(u));
		}

		template <typename Scalar, int Dimension>
		inline Scalar closestPointOnLineSegment(const Eigen::ParametrizedLine<Scalar,Dimension> &_Line, 
			const Eigen::Matrix<Scalar,Dimension,1> &_Point)
		{
			typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType;
			Scalar l = _Line.direction().norm();
			Scalar t = closestPointOnLine(_Line, _Point);
			if (t < 0) { t = 0; }
			else if (t > 1) { t = 1; }
			return t;
		}

		template <typename Scalar, int Dimension>
		inline bool vectorParallelTovector(const Eigen::Matrix<Scalar,Dimension,1> &_DirA, 
			const Eigen::Matrix<Scalar,Dimension,1> &_DirB,
			const double &_Eps /*= 1e-4*/)
		{
			typedef Eigen::Matrix<Scalar, 3, 1> VectorType3D;
			VectorType3D dir_a = common::convertTo3D(_DirA);
			VectorType3D dir_b = common::convertTo3D(_DirB);
			return (dir_a.cross(dir_b).squaredNorm() < _Eps) ? true: false;
		}

		template <typename Scalar, int Dimension>
		inline bool pointOnLine(const Eigen::Matrix<Scalar,Dimension,1> &_Point, 
			const Eigen::ParametrizedLine<Scalar,Dimension> &_Line,
			const double &_Eps /*= 1e-4*/)
		{
			return (sqPointToLineDistance(_Point,_Line) < _Eps)? true:false;
		}

		template <typename Scalar, int Dimension>
		inline bool pointOnLineSegment(const Eigen::Matrix<Scalar,Dimension,1> &_Point, 
			const Eigen::ParametrizedLine<Scalar,Dimension> &_Line,
			const double &_Eps /*= 1e-4*/)
		{
			typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType;
			VectorType point = _Line.origin() + _Line.direction() * closestPointOnLineSegment(_Line, _Point);
			if((_Point - point).squaredNorm() < _Eps)
				return true;
			return false;
		}

		template <typename Scalar, int Dimension>
		inline bool pointInPolygon(const Eigen::Matrix<Scalar,Dimension,1> &_Point, 
			const Polygon<Scalar, Dimension> &_Polygon)
		{
			if(!_Polygon.getBoundingBox().Contains(_Point))
				return false;

			int nvert = _Polygon.getVertexCount();
			int i, j = 0;
			bool odd_nodes = false;
			for (i = 0, j = nvert - 1; i < nvert; j = i++)
			{
				if (((_Polygon[i](1) > _Point(1)) != (_Polygon[j](1) > _Point(1)))
					&& (_Point(0) < (_Polygon[j](0) - _Polygon[i](0)) * (_Point(1) - _Polygon[i](1)) / (_Polygon[j](1) - _Polygon[i](1)) + _Polygon[i](0)))
				{
					odd_nodes = !odd_nodes;
				}
			}
			return odd_nodes;
		}
	};

};